/*
 * HoverCraft.cpp
 *
 *  Created on: Jan 29, 2010
 *      Author: fmason
 */

#include "arduinomega/HardwareSerial.h"
#include "components/HovercraftRadio.h"
#include "components/Hovercraft.h"
#include "constants/command.h"
#include "rtos/os.h"
#include "arduinomega/WProgram.h"
#include <avr/io.h>
#include <avr/interrupt.h>

//function prototypes
void sonar_pp();
void motor_pp();
//tasks
enum tasks
{
	SONARREAD=1, //task 0 is apparently IDLE
	MOTOR=2,
};
const unsigned char PPP[4] = {MOTOR, 200, SONARREAD, 200};
const unsigned int PT = sizeof(PPP)/2;

//states
int const STALLED = -1;
int const STOP = 0;
int const DRIVE = 1;

//decision making constants
int const MAX_WARNINGS = 5;
int const TEST_TIME = 1000;

//resources
int const TEST_LED = 13;

extern "C" void __cxa_pure_virtual(){};

Hovercraft hovercraft;

void r_main()
{
	init();
	Serial.begin( BAUD_RATE );
	hovercraft.setup();

	Serial.println( "Setting up Radio...");
	setupRadioReceiver();
	Serial.println( "Radio setup" );

	hovercraft.setLiftMotorPower(MAX_POWER);

	Task_Create(sonar_pp, 0, PERIODIC, SONARREAD);
	Task_Create(motor_pp, 0, PERIODIC, MOTOR);
}

void sonar_pp(){
	Sonar sonar;
	int distance;
	for(;;){
		Serial.println("loop");
		distance = sonar.getDistance();
		Serial.println(distance);
		Task_Next();
	}
}

void motor_pp(){
	for(;;){
		hovercraft.powerLiftMotor();
		if(hovercraft.getLeftMotorPower() == OFF_DUTY){
			hovercraft.setLeftMotorPower( FULL_DUTY);
		} else{
			hovercraft.setLeftMotorPower( OFF_DUTY);
		}

		Task_Next();
	}
}

void handleCommand( int command )
{
	switch( command )
	{
		case TEST_RADIO:
			pinMode( TEST_LED, OUTPUT );
			digitalWrite( TEST_LED, HIGH );
			Serial.println( "command passed to command handler" );
			break;
		case SET_RIGHT_MOTOR_TO_OFF_DUTY:
			if( hovercraft.getRightMotorPower() == OFF_DUTY )
			{
				break;
			}
			hovercraft.setRightMotorPower( OFF_DUTY );
			//Serial.println( "Set Right motor to Off Duty" );
			break;
		case SET_RIGHT_MOTOR_TO_HALF_DUTY:
			hovercraft.setRightMotorPower( HALF_DUTY );
			//Serial.println( "Set Right motor to half Duty" );
			break;
		case SET_RIGHT_MOTOR_TO_FULL_DUTY:
			hovercraft.setRightMotorPower( FULL_DUTY );
			//Serial.println( "Set Right motor to Full Duty" );
			break;
		case SET_LEFT_MOTOR_TO_OFF_DUTY:
			hovercraft.setLeftMotorPower( OFF_DUTY );
			//Serial.println( "Set Left motor to Off Duty" );
			break;
		case SET_LEFT_MOTOR_TO_HALF_DUTY:
			hovercraft.setLeftMotorPower( HALF_DUTY );
			//Serial.println( "Set Left motor to Half Duty" );
			break;
		case SET_LEFT_MOTOR_TO_FULL_DUTY:
			hovercraft.setLeftMotorPower( FULL_DUTY );
			//Serial.println( "Set Left motor to Full Duty" );
			break;
		case SET_RIGHT_MOTOR_FORWARD:
			hovercraft.setRightMotorDirection( FORWARD );
			//Serial.println( "Set Right Motor to Forward" );
			break;
		case SET_RIGHT_MOTOR_BACKWARD:
			hovercraft.setRightMotorDirection( BACKWARD );
			//Serial.println( "Set Right Motor to Backward" );
			break;
		case SET_LEFT_MOTOR_FORWARD:
			hovercraft.setLeftMotorDirection( FORWARD );
			//Serial.println( "Set Left Motor to Forward" );
			break;
		case SET_LEFT_MOTOR_BACKWARD:
			hovercraft.setLeftMotorDirection( BACKWARD );
			//Serial.println( "Set Left Motor to Backward" );
			break;
		case MOVE_SERVO_TO_RIGHT:
			//Serial.println( "Move Servo to Right" );
			//hovercraft.moveServoToRight();
			break;
		case MOVE_SERVO_TO_LEFT:
			//Serial.println( "Move Servo to Left" );
			//hovercraft.moveServoToLeft();
			break;
		case MOVE_SERVO_TO_MIDDLE:
			//Serial.println( "Move Servo to Middle" );
			//hovercraft.moveServoToMiddle();
			break;
		case SET_LED_COLOUR:
			//Serial.println( "Set LED Color" );
			hovercraft.setLEDColor();
			break;
		default:
			Serial.print( "Received unknown remote command: " );
			Serial.println( command );
			break;
	}
}

